#ifndef __RVIZ_MARKER_H__
#define __RVIZ_MARKER_H__

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

using namespace std;

// https://docs.ros.org/en/api/visualization_msgs/html/msg/Marker.html
// https://docs.ros.org/en/api/visualization_msgs/html/msg/MarkerArray.html
// https://wiki.ros.org/rviz/DisplayTypes/Marker

/********************************* visualization_msgs/Marker *********************************
- ns  Marker的命名空间，用于区分不同类别的 Marker
    示例：marker.ns = "triangle_markers";
    多个不同的Marker可以共享同一个命名空间，但每个命名空间内的Marker需要有唯一的id

- id  Marker的唯一标识符
    示例：marker.id = 0;
    对于同一命名空间中的多个Marker，id必须唯一

- action  控制 Marker 的操作（添加、修改、删除）
    示例：marker.action = visualization_msgs::Marker::ADD;

- lifetime Marker 的生命周期。
    示例：marker.lifetime = ros::Duration(0);
    示例：marker.lifetime = ros::Duration(10.0);  // 10秒后删除
    如果设置为 0，表示 Marker 会一直显示；如果设置为非零值，Marker 会在指定时间后自动消失。

- type  marker类型
    示例：marker.type = visualization_msgs::Marker::TRIANGLE_LIST;

    * 箭头 ARROW=0  
        方式一：
            pose提供箭头的位置和方向；
            scale.x：箭头的总长度，scale.y：箭头的杆部直径，scale.z：箭头头部的高度。
        方式二：
            points存放起点(index 0)和终点(index 1)数据；
            scale.x：箭头杆部的直径，scale.y：箭头头部的直径，scale.z：箭头头部的长度（不为0时）
    * 立方体 CUBE=1
        pose提供立方体的中心位置和方向；
        scale.x, scale.y, scale.z 设置立方体的长度、宽度和高度（以米为单位）
    * 球体 SPHERE=2
        pose提供球体的中心位置；球体没有方向，但orientation需要有效的单位四元数，即marker.pose.orientation.w = 1.0
        scale.x, scale.y, scale.z 设置球体的直径，三个方向的值必须相等，若不相等则会是椭球体
    * 圆柱体 CYLINDER=3
        pose提供圆柱体的中心位置；设置圆柱体的方向（四元数）。圆柱体的默认轴是沿Z轴的，旋转会影响其朝向
        scale.x 和 scale.y 设置圆柱体的直径（两者必须相等）,否则是椭球柱；scale.z 圆柱体的高度

    下面这些列表仍使用visualization_msgs/Marker，不是MarkerArray，是为了加快渲染速度。
    * 连续线段连线 LINE_STRIP = 4   适合展示路径、轨迹或任意连接点的线条
        points 每两个连续的点之间画一条线，如0-1, 1-2, 2-3, 3-4, 4-5 …
        scale.x 线条的宽度（以米为单位）
        color / colors  可选列表的颜色 或 每个点的颜色
    * 独立线段列表 LINE_LIST=5 适合用来显示一系列独立的线段
        points 每两个连续的点之间画一条线，如0-1, 2-3, 4-5, …
        scale.x 线条的宽度（以米为单位）
        color / colors  可选列表的颜色 或 每个点的颜色
    * 立方体列表 CUBE_LIST=6    
        points 每个点都是一个立方体位置
        scale.x, scale.y, scale.z 所有立方体的大小（单位为米）
        color / colors  可选列表的颜色 或 每个立方体的颜色
    * 球体列表 SPHERE_LIST=7
        points 每个点都是一个球体位置
        scale.x, scale.y, scale.z 所有球体的大小（单位为米）
        color / colors  可选列表的颜色 或 每个球体的颜色    
    * 点集列表 POINTS=8
        points 每个点的位置
        scale.x 点的宽度，scale.y为点高, 通常仅设置scale.x
        color / colors  可选列表的颜色 或 每个点的颜色  
    * 文字显示 TEXT_VIEW_FACING=9
        text 显示的文本内容 目前看不显示中文
        pose 文本的位置，旋转不影响文本的朝向，一般保持为默认值
    * 3D模型 MESH_RESOURCE=10
        mesh_resource 3D 模型的资源路径。该路径可以是一个本地文件路径，也可以是一个 ROS 包内的资源路径，通常以 package:// 开头
            本地文件路径："file:///path/to/mesh.obj"
            ROS 包内路径："package://my_robot_description/meshes/model.obj"
        scale.x, scale.y, scale.z  缩放比例，分别对应X、Y、Z 方向的缩放因子
        pose 位姿
        color 颜色
        mesh_use_embedded_materials 为true，则使用模型的颜色/材质，定义的color则无效
    * 三角形列表 TRIANGLE_LIST=11
        points 每3个点被视为一个三角形，如0-1-2, 3-4-5, etc.
        color / colors  可选列表的颜色 或 每个三角形的颜色  

- frame_locked
    如果设置为 true，Marker 将始终固定在 frame_id 所指定的参考坐标系下，不随时间变化
    如果为 false，则 Marker 会随时间移动

************************************************************************************/

class RvizMarker{
    public:
        RvizMarker();
        ~RvizMarker();

        /***************************************************
         * 生成颜色
         ***************************************************/
        // 生成颜色
        std_msgs::ColorRGBA genRGBAColor(float r, float g, float b, float a);
        // 生成颜色 - 蓝色
        std_msgs::ColorRGBA colorBlue(float a = 1.0);
        // 生成颜色 - 绿色
        std_msgs::ColorRGBA colorGreen(float a = 1.0);
        // 生成颜色 - 红色
        std_msgs::ColorRGBA colorRed(float a = 1.0);
        // 生成颜色 - 黄色
        std_msgs::ColorRGBA colorYellow(float a = 1.0);
        // 生成颜色 - 白色
        std_msgs::ColorRGBA colorWhite(float a = 1.0);
        // 生成颜色 - 黑色
        std_msgs::ColorRGBA colorBlack(float a = 1.0);
        // 生成颜色 - 灰色
        std_msgs::ColorRGBA colorGray(float a = 1.0);
        
        /***************************************************
         * 生成尺寸
         ***************************************************/
        // 生成尺寸
        geometry_msgs::Vector3 genScale(float x, float y, float z);


        /***************************************************
         * 生成默认marker
         ***************************************************/  
        // 点集转换
        vector<geometry_msgs::Point> toPoints(vector<geometry_msgs::Pose> poses);
        // 四元数检测
        geometry_msgs::Pose checkQuaternion(geometry_msgs::Pose pose);
        // 生成单位四元数
        geometry_msgs::Pose genDefaultPose();

        // 生成默认箭头Marker ARROW
        visualization_msgs::Marker genArrowMarker(geometry_msgs::Pose arrow_pose);
        visualization_msgs::Marker genArrowMarker(geometry_msgs::Point arrow_start, geometry_msgs::Point arrow_end);
        // 生成默认立方体Marker CUBE
        visualization_msgs::Marker genCubeMarker(geometry_msgs::Pose center_pose, double length);
        // 生成默认球体Marker SPHERE
        visualization_msgs::Marker genSphereMarker(geometry_msgs::Pose center_pose, double radius);
        // 生成默认圆柱体Marker CYLINDER
        visualization_msgs::Marker genCylinderMarker(geometry_msgs::Pose center_pose, double radius, double length);
        // 生成默认连续线段列表Marker LINE_STRIP
        visualization_msgs::Marker genLineStripMarker(vector<geometry_msgs::Pose> poses);
        visualization_msgs::Marker genLineStripMarker(vector<geometry_msgs::Point> points);
        // 生成默认独立线段列表Marker LINE_LIST
        visualization_msgs::Marker genLineListMarker(vector<geometry_msgs::Pose> poses);
        visualization_msgs::Marker genLineListMarker(vector<geometry_msgs::Point> points);        
        // 生成默认立方体列表Marker CUBE_LIST
        visualization_msgs::Marker genCubeListMarker(vector<geometry_msgs::Pose> poses, double length);
        visualization_msgs::Marker genCubeListMarker(vector<geometry_msgs::Point> points, double length);         
        // 生成默认球体列表Marker SPHERE_LIST
        visualization_msgs::Marker genSphereListMarker(vector<geometry_msgs::Pose> poses, double radius);
        visualization_msgs::Marker genSphereListMarker(vector<geometry_msgs::Point> points, double radius);    
        // 生成默认点集列表Marker POINTS
        visualization_msgs::Marker genPointsMarker(vector<geometry_msgs::Pose> poses);
        visualization_msgs::Marker genPointsMarker(vector<geometry_msgs::Point> points);   
        // 生成默认文字显示Marker TEXT_VIEW_FACING
        visualization_msgs::Marker genTextMarker(string text, geometry_msgs::Pose text_pose);
        // 生成默认三角形列表Marker TRIANGLE_LIST
        visualization_msgs::Marker genTriangleListMarker(vector<geometry_msgs::Pose> poses);
        visualization_msgs::Marker genTriangleListMarker(vector<geometry_msgs::Point> points);
        // 生成默认3D模型Marker MESH_RESOURCE
        visualization_msgs::Marker genMeshMarker(string mesh_resource, geometry_msgs::Pose mesh_pose);

        /***************************************************
         * 设置marker属性
         ***************************************************/  
        // 设置坐标系、命名空间、id
        void setMarkerHeader(visualization_msgs::Marker& marker, string frame_id, string ns);
        // 设置ID
        void setMarkerId(visualization_msgs::Marker& marker, int id);
        // 设置尺寸
        void setMarkerScale(visualization_msgs::Marker& marker, geometry_msgs::Vector3 scale);
        // 设置颜色
        void setMarkerColor(visualization_msgs::Marker& marker, std_msgs::ColorRGBA color);

    private:
        string defaultFrameId_;  
        geometry_msgs::Vector3 defaultScale_;
        geometry_msgs::Vector3 defaultPointScale_;
        geometry_msgs::Vector3 defaultLineScale_;
};

#endif